 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
To program robot with open_industrial_ros_controllers
Description: This tutorial shows examples of how to write robot client programs to operate vertical multi-joint industrial arm, in particular DENSO's VS060.Keywords: Denso Wave, MoveIt, Python, Industrial Vertical Arm
Tutorial Level: INTERMEDIATE
Contents
Introduction
DENSO's VS060 package is used as an example in this page. Samples in this page may or may not work with other vertical multi-joint industrial arm robots that run using ROS JointTrajectoryAction, however not tested.
Using MoveIt! via Python
Here are sample codes in python that runs VS060 robot.
Using trajectory MoveIt computes
In the following python code you give starting and destination poses statically, and MoveIt! automatically computes the trajectory in between.
This program can be run with demo_simulation_cage.launch, which can be launched by:
$ roslaunch vs060_moveit_config demo_simulation_cage.launch
Original code found here. Code snippet:
1 #!/usr/bin/env python
2
3 import os
4 from subprocess import check_call
5
6 import actionlib_msgs.msg
7 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
8 from moveit_commander import MoveGroupCommander, conversions
9 from rospkg import RosPack
10 import roslib
11 import rospy
12 import std_msgs.msg
13 from tf.transformations import quaternion_from_euler
14
15 rospy.init_node("test_vs060_moveit")
16
17 g_runnable = False
18 g_prev_status = None
19
20 arm = MoveGroupCommander("manipulator")
21 running_pub = rospy.Publisher("/irex_demo_running", std_msgs.msg.Bool);
22 cancel_pub = rospy.Publisher("/move_group/cancel", actionlib_msgs.msg.GoalID);
23
24 # Get paths for files used later.
25 _rospack = RosPack()
26 SCENE_FILE = _rospack.get_path('vs060') + '/model/irex_model.scene'
27 _path_rosroot = rospy.get_ros_root()
28 _len_cut = len(_path_rosroot) - len('/share/ros') # This is used to get ros root path /opt/%DISTRO% path.
29 _path_rosroot_top = _path_rosroot[:_len_cut]
30 LOAD_SCENE_PROG = _path_rosroot_top + '/lib/vs060/publish_scene_from_text'
31
32 print 'SCENE_FILE=', SCENE_FILE
33 print 'LOAD_SCENE_PROG=', LOAD_SCENE_PROG
34
35 def demo() :
36 # load scene
37 global g_runnable
38 running_pub.publish(std_msgs.msg.Bool(g_runnable))
39 check_call([LOAD_SCENE_PROG, SCENE_FILE])
40 for p in [[ 0.35, -0.35, 0.4],
41 [ 0.6, 0.0, 0.4],
42 [ 0.35, 0.35, 0.4],
43 [ 0.6, 0.0, 0.2],
44 [ 0.4, 0.0, 0.8]]:
45 running_pub.publish(std_msgs.msg.Bool(g_runnable))
46 if g_runnable:
47 print "set_pose_target(", p, ")"
48 pose = PoseStamped(header = rospy.Header(stamp = rospy.Time.now(), frame_id = '/BASE'),
49 pose = Pose(position = Point(*p),
50 orientation = Quaternion(*quaternion_from_euler(1.57, 0, 1.57, 'sxyz'))))
51
52 arm.set_pose_target(pose)
53 arm.go() or arm.go() or rospy.logerr("arm.go fails")
54 rospy.sleep(1)
55 if rospy.is_shutdown():
56 return
57
58 if __name__ == "__main__":
59 while not rospy.is_shutdown():
60 demo()
Take more control over the trajectory using MoveIt!
(TBD)
Using more generic ROS message type with python
Another way to take more control of the trajectory between two poses is to use JointTrajectoryAction directly.
1 #!/usr/bin/env python
2
3 import roslib;
4 roslib.load_manifest('denso_launch')
5 roslib.load_manifest('pr2_controllers_msgs')
6
7 import rospy, actionlib
8 from pr2_controllers_msgs.msg import *
9
10 if __name__ == '__main__':
11 try:
12 rospy.init_node('send_angles', anonymous=True)
13 client = actionlib.SimpleActionClient('/arm_controller/joint_trajectory_action', JointTrajectoryAction)
14 client.wait_for_server()
15
16 goal = pr2_controllers_msgs.msg.JointTrajectoryGoal()
17 goal.trajectory.joint_names.append("j1")
18 goal.trajectory.joint_names.append("j2")
19 goal.trajectory.joint_names.append("j3")
20 goal.trajectory.joint_names.append("j4")
21 goal.trajectory.joint_names.append("j5")
22 goal.trajectory.joint_names.append("flange")
23 print goal.trajectory.joint_names
24
25 point1 = trajectory_msgs.msg.JointTrajectoryPoint()
26 point2 = trajectory_msgs.msg.JointTrajectoryPoint()
27 point1.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
28 point2.positions = [1.5, 1.0, 0.5, 1.0, 1.5, 1.0]
29
30 goal.trajectory.points = [point1, point2]
31
32 goal.trajectory.points[0].time_from_start = rospy.Duration(2.0)
33 goal.trajectory.points[1].time_from_start = rospy.Duration(4.0)
34
35 goal.trajectory.header.stamp = rospy.Time.now()+rospy.Duration(1.0)
36
37 client.send_goal(goal)
38 print client.wait_for_result()
39 except rospy.ROSInterruptException: pass
(TBD line-by-line explanation)
